#include "opencv2/opencv.hpp"
#include "iostream"
#include "fstream"
#include "string"
#include "vector"
#include "algorithm"
#include "list"

using namespace std;
using namespace cv;

vector<vector<float>> a;
vector<float> b;
int length=0;
inline void file_to_string(vector<string> &record, const string& line, char delimiter);
inline float string_to_float(string str);

void read()
{
    vector<string> row;
    string line;
    string filename;
    ifstream in("../cloud_0.csv");
    if (in.fail())  { cout << "File not found" <<endl; return ; }
    while(getline(in, line)  && in.good() )
    {
        length += 1;
        file_to_string(row, line, ',');  //把line里的单元格数字字符提取出来，“,”为单元格分隔符
        for(int i=0, leng=row.size(); i<leng; i++){
            b.push_back(string_to_float(row[i]));
        }
        a.push_back(b);
        b.clear();
    }
    in.close();
    return ;
}

inline void file_to_string(vector<string> &record, const string& line, char delimiter)
{
    int linepos=0;
    char c;
    int linemax=line.length();
    string curstring;
    record.clear();
    while(linepos<linemax)
    {
        c = line[linepos];

        if (isdigit(c) || c == '.' || c == '-') {
            curstring += c;

        }
        else if(c==delimiter && curstring.size()){
            record.push_back(curstring);
            curstring="";
        }
        ++linepos;
    }
    if(curstring.size())
        record.push_back(curstring);
    return;
}

inline float string_to_float(string str){
    int s = 0;
    if (str[0] == '-'){
        str.erase(0,1);
        s = 1;
    }
    int i=0,len=str.length();
    float sum=0;
    while(i<len){
        if(str[i]=='.') break;
        sum=sum*10+str[i]-'0';
        ++i;
    }
    ++i;
    float t=1,d=1;
    while(i<len){
        d*=0.1;
        t=str[i]-'0';
        sum+=t*d;
        ++i;
    }
    if (s==1)
        return -sum;
    else return sum;

}

int main(){
    read();
    double s[16] = {-7.1907391850483116e-03, 1.0494953004635377e-02,
                    9.9991907134097757e-01, 1.0984281510814174e-01,
                    -9.9997142335005851e-01, 2.2580773589691017e-03,
                    -7.2148159989590677e-03, -1.8261670813403203e-02,
                    -2.3336137706425064e-03, -9.9994237686382270e-01,
                    1.0478415848689249e-02, 1.7323651488230618e-01,
                    0., 0., 0., 1. };
    double h[12] = {1.3859739625395162e+03, 0., 9.3622464596653492e+02, 0.,0.,
                   1.3815353250336800e+03, 4.9459467170828475e+02, 0.,0., 0., 1.,0.};
    Mat CameraExtrinsicMat = Mat(4,4,CV_64F,s);
    Mat CameraMat = Mat(3,4,CV_64F,h);
    Mat image_origin = Mat::zeros(1024,1280,CV_8UC3);
    double list_world[4];
    for(int i=0;i<length;i++) {
        for (int j = 0; j < 3; j++) {
            list_world[j] = a[i][j];
        }
        list_world[3] = 1;
        Mat points_world = Mat(4, 1, CV_64F, list_world);
        Mat points_camera = CameraExtrinsicMat * points_world;
        double Zc = points_camera.at<double>(2);
        Mat points_pixel_0 = CameraMat * points_camera;
        Mat points_pixel = points_pixel_0 / Zc;
        Point point_2d;
        point_2d.x = points_pixel.at<double>(1);
        point_2d.y = points_pixel.at<double>(0);
        if (point_2d.x > 0 && point_2d.x < 1280 && point_2d.y > 0 && point_2d.y < 1024) {
            circle(image_origin,point_2d,3,Scalar(255,255,255),-1);
        } else {
            continue;
        }


    }
    //imshow("", image_origin);
    imwrite("4-0.jpg",image_origin);
    waitKey(-1);
}